/*
   Copyright (c) 2016-2017 Slamtec Co., Ltd. All Rights Reserved.
   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at
       http://www.apache.org/licenses/LICENSE-2.0
   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

package com.csjbot.weedle_auto.Slam.agent

import android.content.Context
import android.graphics.RectF
import android.util.Log
import com.csjbot.weedle_auto.Slam.agent.Entity.MapBean
import com.csjbot.weedle_auto.Slam.agent.Util.FileUtil
import com.csjbot.weedle_auto.Slam.data.MapDataCache
import com.csjbot.weedle_auto.Slam.events.*
import com.csjbot.weedle_auto.file.constants_util.Constants
import com.csjbot.weedle_auto.util.Csjlogger
import com.orhanobut.logger.Logger
import com.slamtec.slamware.AbstractSlamwarePlatform
import com.slamtec.slamware.SlamwareCorePlatform
import com.slamtec.slamware.action.ActionStatus
import com.slamtec.slamware.action.IAction
import com.slamtec.slamware.action.IMoveAction
import com.slamtec.slamware.action.MoveDirection
import com.slamtec.slamware.discovery.DeviceManager
import com.slamtec.slamware.geometry.Line
import com.slamtec.slamware.robot.*
import com.slamtec.slamware.robot.Map
import org.greenrobot.eventbus.EventBus
import java.util.*


class RPSlamwareSdpAgent {

    private var robotPlatform_: AbstractSlamwarePlatform? = null

    var robotSlamPlatform: SlamwareCorePlatform? = null
        private set
    val mapData: MapDataCache
    val sweepMapData: MapDataCache

    // robot action info
    @get:Synchronized
    var robotPose: Pose? = null
        private set
    //

    @get:Synchronized
    var laserScan: LaserScan? = null
        private set
    private var moveAction_: IMoveAction? = null
    @get:Synchronized
    var walls: Vector<Line>? = null
        private set
    private var circle_: List<Location>? = null
    private var wallId_: Int = 0

    private var mapType_: MapType? = null
    private var mergedMapUpdateArea_: RectF? = null
    private var targetRotation_: Rotation? = null
    private var wallsToAdd_: Vector<Line>? = null

    var sdpVersion: String? = null
        private set

    private var updatingPose_: Boolean = false
    private var updatingStatus_: Boolean = false
    private var updatingLaserScan_: Boolean = false
    private var updatingMoveAction_: Boolean = false
    private var updatingWalls_: Boolean = false
    private var rotating_: Boolean = false


    private var ip_: String? = null
    private var port_: Int = 0
    private var location_: Location? = null
    private var pose_: Pose? = null
    private var direction_: MoveDirection? = null

    private var shouldReadLocalizationQuliaty = false

    private val worker: Worker

    init {
        mapData = MapDataCache()
        sweepMapData = MapDataCache()

        mergedMapUpdateArea_ = RectF(0f, 0f, 0f, 0f)
        robotPose = Pose(Location(0f, 0f, 0f), Rotation(0f, 0f, 0f))
        port_ = 0
        updatingPose_ = false
        updatingStatus_ = false
        updatingLaserScan_ = false
        updatingMoveAction_ = false
        updatingWalls_ = false
        rotating_ = false
        wallsToAdd_ = Vector()

        sdpVersion = ""
        jobConnect = JobConnect()
        jobAddWalls = JobAddWalls()
        jobCancelAllActions = JobCancelAllActions()
        jobClearMap = JobClearMap()
        jobDisconnect = JobDisconnect()
        jobGoHome = JobGoHome()
        jobClearWalls = JobClearWalls()
        jobMoveBy = JobMoveBy()
        jobMoveTo = JobMoveTo()
        jobMoveToPose = JobMoveToPose()
        jobCircleRun = JobCircleRun()
        jobCircleRun3 = JobCircleRun3()

        jobRegainMap = JobRegainMap()
        jobSweepSpot = JobSweepSpot()
        jobRemoveWallById = JobRemoveWallById()
        jobRotateTo = JobRotateTo()
        jobStartSweep = JobStartSweep()
        jobUpdateLaserScan = JobUpdateLaserScan()
        jobUpdateMap = JobUpdateMap()
        jobUpdateMoveAction = JobUpdateMoveAction()
        jobUpdatePose = JobUpdatePose()
        jobUpdateStatus = JobUpdateStatus()
        jobUpdateWalls = JobUpdateWalls()
        jobUpdateWholeMap = JobUpdateWholeMap()
        jobGetRobotHealth = JobGetRobotHealth()
        jobGoHomePose = JobGoHomePose()
        jobChangeSpeed = JobChangeSpeed()
        jobSaveMap = JobSaveMap()
        worker = Worker()

    }

    val isConnected: Boolean
        get() = connected_

    @Synchronized
    fun getMoveAction(): IMoveAction? {
        return moveAction_
    }

    fun saveMap() {
        pushJob(jobSaveMap)

    }


    fun circleRun(list: List<Location>?) {
        if (list != null) {
            synchronized(this) {
                circle_ = list
                isCircleRunning = true
            }
        }
        Thread(jobCircleRun).start()
        //        Csjlogger.debug("start circle run list size = {}", list.size());
        //        pushJob(jobCircleRun);
    }

    // connection
    fun connectTo(ip: String, port: Int) {
        this.ip_ = ip
        this.port_ = port
        pushJob(jobConnect)
    }


    fun reconnect() {
        if (this.ip_!!.isEmpty() || this.port_ == 0) {
            return
        }
        connectTo(this.ip_!!, this.port_)
    }


    fun disconnect() {
        pushJob(jobDisconnect)
    }


    // robot map info
    fun updateMap(area: RectF) {
        if (area.isEmpty) {
            return
        }

        synchronized(this) {
            if (mergedMapUpdateArea_ != null && !mergedMapUpdateArea_!!.isEmpty) {
                mergedMapUpdateArea_!!.union(area)
                return
            } else {
                mergedMapUpdateArea_ = area
            }
        }
        pushJob(jobUpdateMap)
    }

    //恢复地图
    fun regainMap() {
        pushJob(jobRegainMap)

    }

    fun getMap() {
        if (robotPlatform_ != null) {
            return
        }
        robotPlatform_!!.getMap(mapType_, mergedMapUpdateArea_)
    }

    fun updateMap() {
        pushJob(jobUpdateWholeMap)
    }

    fun updatePose() {
        synchronized(this) {
            if (updatingPose_) {
                return
            }

            updatingPose_ = true
        }

        pushJob(jobUpdatePose)
    }

    fun updateLaserScan() {
        synchronized(this@RPSlamwareSdpAgent) {
            if (updatingLaserScan_) {
                return
            }

            updatingLaserScan_ = true
        }

        pushJob(jobUpdateLaserScan)
    }

    fun updateRobotStatus() {
        synchronized(this@RPSlamwareSdpAgent) {
            if (updatingStatus_)
                return
            updatingStatus_ = true
        }
        pushJob(jobUpdateStatus)
    }

    fun updateMoveAction() {
        synchronized(this@RPSlamwareSdpAgent) {
            if (updatingMoveAction_)
                return

            updatingMoveAction_ = true
        }

        pushJob(jobUpdateMoveAction)
    }

    // robot move action command
    fun moveTo(location: Location) {
        synchronized(this) {
            this.location_ = location
        }
        pushJobHead(jobMoveTo)
    }

    // robot move action command
    fun moveToPose(pose: Pose) {
        synchronized(this) {
            this.pose_ = pose
        }
        Thread(jobMoveToPose).start()
        //        pushJobHead(jobMoveToPose);
    }


    fun moveToHome() {
        pushJobHead(jobGoHome)
    }

    fun rotateTo(rotation: Rotation) {
        synchronized(this@RPSlamwareSdpAgent) {
            targetRotation_ = rotation
            if (rotating_)
                return
            rotating_ = true
        }
        pushJob(jobRotateTo)
    }

    fun moveBy(direction: MoveDirection) {
        synchronized(this) {
            this.direction_ = direction
        }
        pushJob(jobMoveBy)
    }

    fun sweepSpot(location: Location) {
        synchronized(this) {
            this.location_ = location
        }
        pushJobHead(jobSweepSpot)
    }

    // clear map
    fun clearMap() {
        pushJobHead(jobClearMap)
    }

    // robot map command
    fun updateWalls() {
        synchronized(this@RPSlamwareSdpAgent) {
            if (updatingWalls_)
                return
            updatingWalls_ = true
        }
        pushJob(jobUpdateWalls)
    }

    fun addWall(line: Line) {
        synchronized(this) {
            wallsToAdd_!!.add(line)
            if (wallsToAdd_!!.size != 1)
                return
        }
        pushJob(jobAddWalls)
    }

    fun addWalls(lines: Vector<Line>) {
        synchronized(this@RPSlamwareSdpAgent) {
            if (wallsToAdd_!!.isEmpty()) {
                wallsToAdd_ = lines
            } else {
                wallsToAdd_!!.addAll(lines)
                return
            }
        }
        pushJob(jobAddWalls)
    }

    fun clearWalls() {
        pushJob(jobClearWalls)
    }

    fun removeWallById(wallId: Int) {
        synchronized(this) {
            wallId_ = wallId
        }
        pushJob(jobRemoveWallById)
    }

    fun startSweep() {
        pushJobHead(jobStartSweep)
    }

    fun goHome() {
        pushJobHead(jobGoHome)
    }

    fun changeSpeed() {
        Thread(jobChangeSpeed).run()
    }

    fun cancelAllActions() {
        pushJobHead(jobCancelAllActions)
    }

    fun getRobotHealth() {
        pushJob(jobGetRobotHealth)
    }

    @Synchronized private fun pushJobHead(job: Runnable) {
        worker.pushHead(job)
    }

    @Synchronized private fun pushJob(job: Runnable) {
        worker.push(job)
    }

    private fun onRequestError(e: Exception) {
        synchronized(this) {
            worker.clear()
            mapData.clear()
            sweepMapData.clear()
            robotPlatform_ = null
            connected_ = false
        }
        //        Csjlogger.error("connect to slam error, will post event", e);
        EventBus.getDefault().post(ConnectionLostEvent(e.message!!))
    }

    /**
     * 停止循环送餐
     */
    fun stopCircleRun() {
        synchronized(this) {
            isCircleRunning = false
            if (circleAction != null) {
                circleAction!!.cancel()
                circleAction = null
            }
            cancelAllActions()
        }
    }

    /**
     * 停止送餐
     */
    fun stopSendFoodRun() {
        synchronized(this) {
            isFirstActionDoing = false
            isfinished = false
            if (moveAction != null) {
                moveAction!!.cancel()
                moveAction = null
            }
            cancelAllActions()
        }
    }

    private inner class JobCircleRun : Runnable {

        override fun run() {
            var platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
            }
            // 设置参数 MoveOption
            val opt = MoveOption()
            opt.isAppending = true
            opt.isMilestone = true
            opt.isKeyPoints = true

            // 初始化判断变量
            var index = 0
            val max = circle_!!.size
            // 第一个点
            var location = circle_!![index]
            circleAction = platform.moveTo(location, opt)
            var pose = robotPlatform_!!.pose//调试，获取当前位置
            Csjlogger.debug("JobCircleRun action = {} actionid = {} start status = {} cur pose = {} {} move to pose = {} {}", circleAction!!.actionName, circleAction!!.actionId, circleAction!!.status,
                    pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())
            circleAction!!.waitUntilDone()
            while (isCircleRunning) {
                if (circleAction != null) {
                    Csjlogger.debug("JobCircleRun action = {} actionid = {} while status = {}", circleAction!!.actionName, circleAction!!.actionId, circleAction!!.status)
                    if (circleAction!!.status == ActionStatus.FINISHED) {
                        Csjlogger.debug("JobCircleRun finished index={}", index)
                        index++
                        if (index >= max) {
                            index = 0
                        }
                        location = circle_!![index]
                        circleAction = platform.moveTo(location, opt)
                        pose = robotPlatform_!!.pose//调试，获取当前位置
                        Csjlogger.debug("JobCircleRun action = {} actionid = {} finished status = {} cur pose = {} {} move to pose = {} {}", circleAction!!.actionName, circleAction!!.actionId, circleAction!!.status,
                                pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())
                        circleAction!!.waitUntilDone()
                    } else {
                        circleAction = platform.moveTo(location, opt)
                        pose = robotPlatform_!!.pose//调试，获取当前位置
                        Csjlogger.debug("JobCircleRun action = {} actionid = {} error status = {} cur pose = {} {} move to pose = {} {}", circleAction!!.actionName, circleAction!!.actionId, circleAction!!.status,
                                pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())
                        circleAction!!.waitUntilDone()
                    }
                }
            }
        }
    }

    private inner class JobCircleRun2 : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
            }
            if (robotPlatform_ == null) {
                return
            }
            // 设置参数 MoveOption
            val opt = MoveOption()
            opt.isAppending = true
            opt.isMilestone = true
            opt.isKeyPoints = true

            // 初始化判断变量
            var index = 0
            val max = circle_!!.size
            // 第一个点
            var location = circle_!![index]
            var pose = platform.pose//调试，获取当前位置

            var action: IMoveAction? = platform.moveTo(location, opt)
            Csjlogger.debug("JobCircleRun action = {} status = {} cur pose = {} {} move to pose = {} {}", action!!.actionName, action.status,
                    pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())

            while (isCircleRunning) {
                Csjlogger.debug("JobCircleRun action = {} status = {} cur pose = {} {} move to pose = {} {}", action!!.actionName, action.status,
                        pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())
                if (action.status == ActionStatus.ERROR) {
                    Csjlogger.debug("JobCircleRun ERROR index={}", index)
                    try {
                        Thread.sleep(300)
                    } catch (e: InterruptedException) {
                        e.printStackTrace()
                    }

                    action = platform.moveTo(location, opt)
                    pose = robotPlatform_!!.pose//调试，获取当前位置
                    Csjlogger.debug("JobCircleRun action = {} status = {} cur pose = {} {} move to pose = {} {}", action!!.actionName, action.status,
                            pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())
                }
                if (action.status != ActionStatus.FINISHED) {
                    Csjlogger.debug("JobCircleRun not finished index={}", index)
                    try {
                        Thread.sleep(500)
                    } catch (e: InterruptedException) {
                        e.printStackTrace()
                    }

                } else {
                    Csjlogger.debug("JobCircleRun finished index={}", index)
                    index++
                    if (index >= max) {
                        index = 0
                    }

                    location = circle_!![index]
                    action = platform.moveTo(location, opt)
                    pose = robotPlatform_!!.pose//调试，获取当前位置
                    Csjlogger.debug("JobCircleRun action = {} status = {} cur pose = {} {} move to pose = {} {}", action!!.actionName, action.status,
                            pose.x.toString(), pose.y.toString(), location.x.toString(), location.y.toString())
                }
            }
        }
    }

    /**
     * puyz add
     */
    private var isCircleRunning = true
    private var circleAction: IMoveAction? = null

    private inner class JobCircleRun3 : Runnable {
        /**
         * When an object implementing interface `Runnable` is used
         * to create a thread, starting the thread causes the object's
         * `run` method to be called in that separately executing
         * thread.
         *
         *
         * The general contract of the method `run` is that it may
         *
         * take any action whatsoever.
         *
         * @see Thread.run
         */
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
            }
            Csjlogger.debug("start circle run platform = {}")
            if (robotPlatform_ == null) {
                return
            }
            // 设置参数 MoveOption
            val opt = MoveOption()
            opt.isAppending = true
            opt.isMilestone = true
            opt.isKeyPoints = true

            // 初始化判断变量
            var index = 0
            val max = circle_!!.size
            var lastPointSize = 0
            // 第一个点
            var location = circle_!![index]

            var action: IMoveAction? = platform.moveTo(location, opt)
            Csjlogger.debug("start circle run action = {}", action!!.actionName)
            while (isCircleRunning) {
                Csjlogger.debug("start circle isCircleRunning {} action11111 = {}", isCircleRunning, action!!.actionName)
                // 取剩余的点
                val leftPointSize = action.remainingPath.points.size
                val remainingMilestones = action.remainingMilestones.points.size
                // 先判断任务是否被取消
                Csjlogger.debug("start circle leftPointSize == {} lastPointSize == {}, index == {}", leftPointSize, lastPointSize, index)
                if (lastPointSize - leftPointSize > 2) {
                    // 理论上说 lastPointSize - leftPointSize 应该等于1（除了第一次）
                    // 但是有特殊情况，
                    // 如果大于2，则说明路径突然没有了，任务被取消了
                    // 继续走上一个点
                    action = platform.moveTo(location, opt)
                    //                        if (CommonUtil.floatEquals(location.getX(), firstLocation.getX()) && CommonUtil.floatEquals(location.getY(), firstLocation.getY())
                    //                                && CommonUtil.floatEquals(location.getZ(), firstLocation.getZ())) {//如果是第一个点就发eventbus，通知可以进行循环送餐录像的开始结束任务了
                    //                            EventBus.getDefault().post(new CircleCameraRecordingEvents());
                    //                        }
                    // TODO 打印
                } else {
                    lastPointSize = leftPointSize
                    if (remainingMilestones < 2) {//待定2/3
                        // 剩余的点是否 <= 1，如果小于等于 1， 则需要走下面一个点了
                        if (leftPointSize <= 1) {
                            index++
                            if (index >= max) {
                                index = 0
                            }

                            location = circle_!![index]
                            action = platform.moveTo(location, opt)
                        } else {
                            //                            EventBus.getDefault().post(new AutoRunPath(leftPointSize));
                            //TODO: 2017/3/30 打印信息
                            Csjlogger.debug("start circle 剩余路径点 " + leftPointSize)
                        }
                    }
                }
                try {
                    Thread.sleep(300)
                } catch (e: InterruptedException) {
                    e.printStackTrace()
                }

            }
        }
    }

    private inner class JobSaveMap : Runnable {

        override fun run() {
            Logger.d("chenqi saveMap")
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            var mapType: MapType? = null
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
                mapType = mapType_
            }
            try {
                val knownArea = platform.getKnownArea(mapType)
                val map: Map
                map = platform.getMap(mapType, knownArea)
                val mapBean = MapBean()
                mapBean.setMap(map, FileUtil.SaveWalls(platform), FileUtil.SaveTrack(platform))
                FileUtil.saveMapToFile(mapBean)
            } catch (e: Exception) {
                Logger.d("chenqi  save failed")
                e.printStackTrace()
            }

        }
    }

    private inner class JobConnect : Runnable {
        override fun run() {
            try {
                val ip = Constants.SLAM_IP
                val port = Constants.SLAM_PORT
                val robotPlatform = SlamwareCorePlatform.connect(ip, port)
                val mapTypes = robotPlatform.availableMaps
                val mapType = mapTypes[0]


                // RectF knownArea = robotPlatform.getKnownArea(mapType);
                val currentLaserScan = robotPlatform.laserScan
                val sdpVersion = robotPlatform.slamwareVersion

                shouldReadLocalizationQuliaty = false

                val versions = sdpVersion.split("[.]".toRegex()).dropLastWhile { it.isEmpty() }.toTypedArray()
                if (versions.size >= 2) {
                    if (versions[0].toInt() > 1) {
                        shouldReadLocalizationQuliaty = true
                    } else if (versions[0].toInt() == 1 && Integer.valueOf(versions[1]) > 7) {
                        shouldReadLocalizationQuliaty = true
                    } else if (versions.size >= 3 && versions[0].toInt() == 1 &&
                            versions[1].toInt() == 7 && versions[2].toInt() >= 1) {
                        shouldReadLocalizationQuliaty = true
                    }
                }

                robotPlatform.mapUpdate = false//不扫和定位地图

                synchronized(this@RPSlamwareSdpAgent) {
                    robotPlatform_ = robotPlatform
                    robotSlamPlatform = robotPlatform
                    mapType_ = mapType
                    laserScan = currentLaserScan
                    connected_ = true
                }
                //                RectF clipArea = new RectF(
                //                        (float) (robotPose.getLocation().getX() - SLAMWARE_MAX_UPDATE_MAP_RADIX * 2),
                //                        (float) (robotPose.getLocation().getY() - SLAMWARE_MAX_UPDATE_MAP_RADIX * 2),
                //                        (float) (robotPose.getLocation().getX() + SLAMWARE_MAX_UPDATE_MAP_RADIX * 2),
                //                        (float) (robotPose.getLocation().getY() + SLAMWARE_MAX_UPDATE_MAP_RADIX * 2)
                //                );
                //                knownArea.intersect(clipArea);

                //updateMap(knownArea);
            } catch (exception: Exception) {
                onRequestError(exception)
                return
            }

            Csjlogger.debug("connect slam success, will post event")
            EventBus.getDefault().post(ConnectedEvent("success"))
        }
    }

    private inner class JobDisconnect : Runnable {
        override fun run() {
            synchronized(this@RPSlamwareSdpAgent) {
                if (robotPlatform_ == null) {
                    return
                }
                worker.clear()
                mapData.clear()
                sweepMapData.clear()
                robotPlatform_!!.disconnect()
                robotPlatform_ = null
                connected_ = false
            }
        }
    }

    /**
     * 恢复地图，这个方法是不管你有没有恢复的地图的情况下都会进行重新加载地图。
     * 所以谨慎使用。
     */
    private inner class JobRegainMap : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
            }
            if (robotPlatform_ == null || FileUtil.read() == null) {
                return
            }
            try {
                val map = FileUtil.read() as MapBean//
                platform.setMap(map.map)
                platform.addLines(ArtifactUsage.ArtifactUsageVirutalWall, FileUtil.getWall(map))
                platform.addLines(ArtifactUsage.ArtifactUsageVirtualTrack, FileUtil.getTrack(map))
                //保存虚拟墙
                Csjlogger.info("chenqi  恢复成功" + map.timestamp + map.data)
            } catch (e: Exception) {
                Csjlogger.info("chenqi 恢复地图")
                e.printStackTrace()
            }

            EventBus.getDefault().post(MapOperationEvent(MapOperationEvent.REGAINMAPOK))
        }
    }

    private inner class JobUpdateMap : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            val area = RectF()

            synchronized(this@RPSlamwareSdpAgent) {
                val tmp = RectF(mergedMapUpdateArea_)
                mergedMapUpdateArea_!!.set(area)
                area.set(tmp)
                robotPlatform_ = platform
            }

            if (robotPlatform_ == null) {
                return
            }

            if (area.isEmpty) {
                EventBus.getDefault().post(MapUpdateEvent(area))
                return
            }

            val map: Map

            try {
                map = platform.getMap(mapType_, MapKind.EXPLORE_MAP, area)
            } catch (e: Exception) {
                Logger.d("chenqi -------------------" + e.message)
                onRequestError(e)
                return
            }

            synchronized(this@RPSlamwareSdpAgent) {
                mapData.update(map)
            }

            EventBus.getDefault().post(MapUpdateEvent(area))
        }
    }

    private inner class JobUpdateWholeMap : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)

            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
            }

            if (robotPlatform_ == null) {
                return
            }

            try {
                val area = platform.getKnownArea(mapType_)
                updateMap(area)
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }

    private inner class JobUpdatePose : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
                if (robotPlatform_ == null) {
                    updatingPose_ = false
                    return
                }
            }

            val pose: Pose

            try {
                pose = platform.pose
                synchronized(this@RPSlamwareSdpAgent) {
                    robotPose = pose
                    updatingPose_ = false
                }
            } catch (e: Exception) {
                Log.e("chenqi", "update pose exception")
                onRequestError(e)
                updatingPose_ = false
                return
            }

            EventBus.getDefault().post(RobotPoseUpdateEvent(pose))
        }
    }

    private inner class JobUpdateLaserScan : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
                if (robotPlatform_ == null) {
                    updatingLaserScan_ = false
                    return
                }
            }

            val laserScan1: LaserScan? = null
            var pose: Pose? = null

            try {
                laserScan = platform.laserScan
                if (laserScan1 != null) {
                    pose = laserScan1.pose
                }
            } catch (e: Exception) {
                onRequestError(e)
                updatingLaserScan_ = false
                return
            }

            synchronized(this@RPSlamwareSdpAgent) {
                laserScan = laserScan1
                if (laserScan != null && pose != null) {
                    robotPose = pose
                }
                updatingLaserScan_ = false
            }

            EventBus.getDefault().post(LaserScanUpdateEvent(laserScan1!!))
        }
    }

    private inner class JobUpdateStatus : Runnable {
        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this@RPSlamwareSdpAgent) {
                robotPlatform_ = platform
                if (robotPlatform_ == null) {
                    updatingStatus_ = false
                    return
                }
            }

            val batteryPercentage: Int
            val isCharging: Boolean
            val localizationQuality: Int

            try {
                batteryPercentage = platform.batteryPercentage
                isCharging = platform.batteryIsCharging
                localizationQuality = if (shouldReadLocalizationQuliaty) platform.localizationQuality else -1
                getRobotHealth()
            } catch (e: Exception) {
                onRequestError(e)
                updatingStatus_ = false
                return
            }

            synchronized(this@RPSlamwareSdpAgent) {
                updatingStatus_ = false
            }
            EventBus.getDefault().post(RobotStatusUpdateEvent(batteryPercentage, isCharging, localizationQuality))
        }
    }

    private inner class JobUpdateMoveAction : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                if (platform == null) {
                    updatingMoveAction_ = false
                    return
                }
            }

            val moveAction: IMoveAction?

            try {
                moveAction = platform!!.currentAction
                if (moveAction != null && OperateAction.shouldStop(moveAction.actionName)) {
                    moveAction.cancel()
                    OperateAction.reset()
                }
            } catch (e: Exception) {
                onRequestError(e = e)
                updatingMoveAction_ = false
                return
            }

            synchronized(this@RPSlamwareSdpAgent) {
                moveAction_ = moveAction
                updatingMoveAction_ = false
            }
        }
    }


    private inner class JobChangeSpeed : Runnable {

        override fun run() {
            val platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            synchronized(this) {
                robotPlatform_ = platform
            }
            EventBus.getDefault().post(RobotChangeSpeedEvents(true))
        }
    }

    private var isFirstActionDoing = true
    private val time = 5000
    private var isfinished = true
    private var moveAction: IMoveAction? = null

    /**
     *
     */
    private inner class JobMoveToPose : Runnable {

        override fun run() {
            Csjlogger.info("pose set to slam start")
            var pose: Pose? = null
            var platform: AbstractSlamwarePlatform = DeviceManager.connect(ip_, port_)
            isFirstActionDoing = true

            synchronized(this@RPSlamwareSdpAgent) {
                pose = pose_
                robotPlatform_ = platform
            }

            Csjlogger.DBL()

            if (pose == null || robotSlamPlatform == null) {
                Csjlogger.info("pose set to slam start robotSlamPlatform_ null")
                return
            }
            isfinished = true
            var Times = 0

            try {
                val location = pose!!.location
                val moveOption = MoveOption()
                moveOption.isKeyPoints = true
                moveOption.isPrecise = true//解决机器人无法完全运动到目标点的问题
                //                moveOption.setWithYaw(true);//添加朝向
                // TODO: 2017/4/12 验证这个方法
                //                moveOption.setYaw(pose.getYaw());
                moveAction = platform.moveTo(location, moveOption)
                Csjlogger.debug("RPSlamwareSdpAgent JobMoveToPose action = {} actionid = {} start status = {} move to pose = {} {}", moveAction!!.actionName, moveAction!!.actionId, moveAction!!.status,
                        location.x.toString(), location.y.toString())
                while (isFirstActionDoing) {
                    if (moveAction != null) {
                        moveAction!!.waitUntilDone()
                        Csjlogger.debug("RPSlamwareSdpAgent JobMoveToPose action = {} actionid = {} while status = {} move to pose = {} {}", moveAction!!.actionName, moveAction!!.actionId, moveAction!!.status,
                                location.x.toString(), location.y.toString())
                        if (moveAction!!.status == ActionStatus.FINISHED) {
                            isFirstActionDoing = false
                        } else {
                            Csjlogger.debug("RPSlamwareSdpAgent JobMoveToPose action = {} actionid = {} not finish status = {} move to pose = {} {}", moveAction!!.actionName, moveAction!!.actionId, moveAction!!.status,
                                    location.x.toString(), location.y.toString())
                            moveAction = platform.moveTo(location, moveOption)
                            moveAction!!.waitUntilDone()
                        }
                    }
                }

                Csjlogger.debug("RPSlamwareSdpAgent JobRotateTo action = {} actionid = {} start status = {} move to pose = {}", moveAction!!.actionName, moveAction!!.actionId, moveAction!!.status,
                        pose!!.rotation.toString())
                moveAction = platform.rotateTo(pose!!.rotation)
                moveAction!!.waitUntilDone()
                while (isfinished) {
                    // 这个是检查到达 Location 之后的转身
                    Csjlogger.debug("RPSlamwareSdpAgent JobRotateTo action = {} actionid = {} while status = {} move to pose = {}", moveAction!!.actionName, moveAction!!.actionId, moveAction!!.status,
                            pose!!.rotation.toString())
                    if (moveAction!!.status == ActionStatus.FINISHED) {
                        moveAction!!.cancel()
                        moveAction = null
                        isfinished = false
                        EventBus.getDefault().post(ArriveHomeEvet())
                    } else {
                        Csjlogger.debug("RPSlamwareSdpAgent JobRotateTo action = {} actionid = {} not finish status = {} move to pose = {} time = {}", moveAction!!.actionName, moveAction!!.actionId, moveAction!!.status,
                                pose!!.rotation.toString(), Times)
                        if (Times < 3) {
                            Times++
                            moveAction = platform.rotateTo(pose!!.rotation)
                            moveAction!!.waitUntilDone()
                        } else {
                            moveAction!!.cancel()
                            moveAction = null
                            isfinished = false
                            EventBus.getDefault().post(ArriveHomeEvet())
                        }
                    }
                }
            } catch (e: Exception) {
                onRequestError(e)
            }

            EventBus.getDefault().post(SendPoseFiled())
        }
    }

    private inner class JobMoveTo : Runnable {

        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            var location: Location? = null
            val moveOption = MoveOption()
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                location = location_
                if (platform == null || location == null)
                    return
            }

            try {

                platform!!.moveTo(location, moveOption)
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }

    private inner class JobSweepSpot : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            var location: Location? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                location = location_
                if (platform == null || location == null) {
                    return
                }
            }

            try {
                platform!!.sweepSpot(location)
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }

    private inner class JobRotateTo : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            var targetRotation: Rotation? = null

            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                if (platform == null) {
                    rotating_ = false
                    return
                }
                targetRotation = targetRotation_
            }

            try {
                platform!!.rotateTo(targetRotation)
            } catch (e: Exception) {
                onRequestError(e)
            }

            rotating_ = false
        }
    }

    private inner class JobMoveBy : Runnable {

        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            var direction: MoveDirection? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                direction = direction_
                if (platform == null || direction == null) {
                    return
                }
            }
            try {
                platform!!.moveBy(direction)
            } catch (e: Exception) {
                onRequestError(e)
            }

            synchronized(this@RPSlamwareSdpAgent) {
                direction_ = null
            }
        }
    }

    private inner class JobClearMap : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null

            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
            }

            if (platform == null)
                return

            try {
                platform!!.clearMap()
            } catch (e: Exception) {
                onRequestError(e)
                return
            }

            synchronized(this@RPSlamwareSdpAgent) {
                mapData.clear()
                sweepMapData.clear()
            }

            EventBus.getDefault().post(MapUpdateEvent(RectF(-60f, -60f, 60f, 60f)))
        }
    }

    private inner class JobUpdateWalls : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                if (platform == null) {
                    updatingWalls_ = false
                    return
                }
            }

            val walls1: Vector<Line>

            try {
                walls1 = platform!!.walls
            } catch (e: Exception) {
                onRequestError(e)
                updatingWalls_ = false
                return
            }

            synchronized(this@RPSlamwareSdpAgent) {
                walls = walls1
            }

            updatingWalls_ = false
            EventBus.getDefault().post(WallUpdateEvent(walls1))
        }
    }

    private inner class JobAddWalls : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            val wallsToAdd = Vector<Line>()

            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                if (platform == null)
                    return
                val tmp = Vector(wallsToAdd_!!)
                wallsToAdd_!!.clear()
                wallsToAdd.clear()
                wallsToAdd.addAll(tmp)
            }

            if (wallsToAdd.isEmpty())
                return

            try {
                platform!!.addWalls(wallsToAdd)
            } catch (e: Exception) {
                onRequestError(e)
            }

            updateWalls()
        }
    }

    private inner class JobClearWalls : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                if (platform == null)
                    return
            }
            try {
                platform!!.clearWalls()
            } catch (e: Exception) {
                onRequestError(e)
            }

            updateWalls()
        }
    }

    private inner class JobRemoveWallById : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            var wallId: Int? = 0

            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                if (platform == null) {
                    return
                }
                wallId = wallId_
            }

            try {
                platform!!.clearWallById(wallId!!)
            } catch (e: Exception) {
                onRequestError(e)
            }

            updateWalls()
        }
    }

    private inner class JobStartSweep : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
            }

            if (platform == null)
                return

            try {
                platform!!.startSweep()
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }


    private inner class JobGoHomePose : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            var pose: Pose? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
                pose = pose_
            }

            if (platform == null || pose == null) {
                return
            }
            var isfinished = true
            var Times = 0
            Logger.d("Move To Pose is run")
            try {
                val action = platform!!.goHome()
                action.waitUntilDone()
                val goHome = platform!!.rotateTo(pose!!.rotation)
                while (isfinished) {
                    Thread.sleep(300)
                    if (Times < 3) {
                        if (goHome.status == ActionStatus.FINISHED) {
                            Times++
                        } else if (goHome.status == ActionStatus.RUNNING) {
                            Times = 0
                        }
                    } else if (Times > 3 || Times == 3) {
                        isfinished = false
                        EventBus.getDefault().post(ArriveHomeEvet())
                        Times = 0
                        Logger.d("chenqi run is not or is over" + goHome.status.toString())
                    }
                    // todo  print move2PoseAction.getStatus().toSting();
                    Thread.sleep(500)
                }
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }

    private inner class JobGoHome : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
            }
            if (platform == null)
                return
            var isfinished = true
            var Times = 0
            Logger.d("Move To Pose is run")
            try {
                var action: IAction = platform!!.goHome()
                action.waitUntilDone()
                while (isfinished) {
                    if (action.status == ActionStatus.FINISHED) {
                        Csjlogger.debug("RPSlamwareSdpAgent gohome success")
                        isfinished = false
                        Times = 0
                    } else if (Times <= 3) {
                        Csjlogger.debug("RPSlamwareSdpAgent gohome ing Time={}", Times)
                        Times++
                        action = platform!!.goHome()
                        action.waitUntilDone()
                    } else {
                        Csjlogger.debug("RPSlamwareSdpAgent gohome Fail")
                        isfinished = false
                    }
                }
                EventBus.getDefault().post(ArriveHomeEvet())//不管充电是否成功，都发该eventbus
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }

    private inner class JobCancelAllActions : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            val moveAction: IMoveAction?
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
            }
            if (platform == null) {
                return
            }

            try {
                moveAction = platform!!.currentAction
                moveAction?.cancel()
            } catch (e: Exception) {
                onRequestError(e)
            }

            worker.clear()

            synchronized(this@RPSlamwareSdpAgent) {
                updatingStatus_ = false
                updatingMoveAction_ = false
                updatingLaserScan_ = false
                updatingPose_ = false
                updatingWalls_ = false
                rotating_ = false
            }
            EventBus.getDefault().post(ActionCancelEvent(true))
        }
    }

    private inner class JobGetRobotHealth : Runnable {
        override fun run() {
            var platform: AbstractSlamwarePlatform? = null
            synchronized(this@RPSlamwareSdpAgent) {
                platform = robotPlatform_
            }

            if (platform == null) {
                return
            }

            try {
                val info = platform!!.robotHealth
                EventBus.getDefault().post(RobotHealthInfoEvent(info))
            } catch (e: Exception) {
                onRequestError(e)
            }

        }
    }

    companion object {
        private val TAG = "RPSlamwareSdpAgent"

        private val SLAMWARE_MAX_UPDATE_MAP_RADIX = 7.0f

        private var connected_: Boolean = false
        private lateinit var jobConnect: JobConnect
        private lateinit var jobAddWalls: JobAddWalls
        private lateinit var jobCancelAllActions: JobCancelAllActions
        private lateinit var jobClearMap: JobClearMap
        private lateinit var jobDisconnect: JobDisconnect
        private lateinit var jobGoHome: JobGoHome
        private lateinit var jobClearWalls: JobClearWalls
        private lateinit var jobMoveBy: JobMoveBy
        private lateinit var jobMoveTo: JobMoveTo
        private lateinit var jobMoveToPose: JobMoveToPose
        private lateinit var jobSweepSpot: JobSweepSpot
        private lateinit var jobRemoveWallById: JobRemoveWallById
        private lateinit var jobRotateTo: JobRotateTo
        private lateinit var jobGoHomePose: JobGoHomePose
        private lateinit var jobStartSweep: JobStartSweep
        private lateinit var jobUpdateLaserScan: JobUpdateLaserScan
        private lateinit var jobUpdateMap: JobUpdateMap
        private lateinit var jobUpdateMoveAction: JobUpdateMoveAction
        private lateinit var jobUpdatePose: JobUpdatePose
        private lateinit var jobUpdateStatus: JobUpdateStatus
        private lateinit var jobUpdateWalls: JobUpdateWalls
        private lateinit var jobUpdateWholeMap: JobUpdateWholeMap
        private lateinit var jobGetRobotHealth: JobGetRobotHealth
        private lateinit var jobSaveMap: JobSaveMap
        private lateinit var jobRegainMap: JobRegainMap
        private lateinit var jobCircleRun: JobCircleRun
        private lateinit var jobCircleRun3: JobCircleRun3
        private lateinit var jobChangeSpeed: JobChangeSpeed

        private val rpSlamwareSdpAgent: RPSlamwareSdpAgent? = null

        private var agent: RPSlamwareSdpAgent? = null

        private var context: Context? = null

        fun getAgent(cxt: Context): RPSlamwareSdpAgent {
            if (agent == null) {
                agent = RPSlamwareSdpAgent()
                context = cxt
            }

            return agent as RPSlamwareSdpAgent
        }
    }
}
